/// \file   frame_main.cpp
/// \brief  A main file for transformation testing.

#include "turtlelib/rigid2d.hpp"

#include <cmath>
#include <iostream>
#include <string>
#include <vector>

using namespace turtlelib;
/// \brief A main function for transformation testing.
int main()
{
  Vector2D v_a, v_b, v_c;
  Twist2D twist_a, twist_b, twist_c;
  Transform2D Tab, Tbc, Tba, Tcb, Tac, Tca;

  // Prompt the user to enter transforms Tab and Tbc:
  std::cout << "Enter transform T_{a,b}: " << std::endl;
  std::cin >> Tab;
  std::cout << "Enter transform T_{b,c}: " << std::endl;
  std::cin >> Tbc;

  // Compute and output the transforms Tab, Tba, Tbc, Tcb, Tac, Tca:
  std::cout << "T_{a,b}:" << Tab << std::endl;
  Tba = Tab.inv();
  std::cout << "T_{b,a}:" << Tba << std::endl;
  std::cout << "T_{b,c}: " << Tbc << std::endl;
  Tcb = Tbc.inv();
  std::cout << "T_{c,b}:" << Tcb << std::endl;
  Tac = Tab * Tbc;
  std::cout << "T_{a,c}:" << Tac << std::endl;
  Tca = Tac.inv();
  std::cout << "T_{c,a}:" << Tca << std::endl;

  // Prompt the user to enter a vector and the frame in which it is defined:
  std::cout << "Enter Vector v_b: " << std::endl;
  std::cin >> v_b;
  std::cout << "v_bhat: " << v_b.normalize() << std::endl;
  v_a = Tab(v_b);
  std::cout << "v_a: " << v_a << std::endl;
  std::cout << "v_b: " << v_b << std::endl;
  v_c = Tcb(v_b);
  std::cout << "v_c: " << v_c << std::endl;

  std::cout << "Enter twist V_b: " << std::endl;
  std::cin >> twist_b;

  twist_a = Tab(twist_b);
  std::cout << "V_a " << twist_a << std::endl;
  std::cout << "V_b " << twist_b << std::endl;
  twist_c = Tcb(twist_b);
  std::cout << "V_c " << twist_c << std::endl;

  return 0;
}
